clc;
clear;
close all;

addpath('autogen\')

% q=[-1.62439, -1.71653, 1.05723, 0.0218965]
%tau_est=  -2.00693   -1.72651 -0.0631895   0.168412

% 1、验证DH参数 passed
%{
a=[0, 0, 0.00004, 0];
alpha = [-1.309, -pi/2, pi/2, -pi/2];
d = [0, 0.0145, 0.38804, 0];
q = [-pi/2, -pi/2-0.1658, 0, 0];

for i=1:4
    T(:,:,i)=[
            cos(q(i)), -sin(q(i)), 0, a(i);
            sin(q(i)).*cos(alpha(i)), cos(q(i))*cos(alpha(i)), -sin(alpha(i)), -d(i)*sin(alpha(i));
            sin(q(i))*sin(alpha(i)), cos(q(i))*sin(alpha(i)), cos(alpha(i)), d(i)*cos(alpha(i));
            0,0,0,1];
end

% 在axis_0坐标系下某一点左边
P = [-0.37909; 0.048; 0.28666; 1];
T01=T(:,:,1);
T02=T01*T(:,:,2);
T03=T02*T(:,:,3);
T04=T03*T(:,:,4);
% 转换到axis_4坐标系下，检测坐标变换是否正确？
newP = inv(T04)*P;
%}

% 2、生成雅可比函数
%{
a=[0, 0, 0.00004, 0];
d = [0, 0.0145, 0.38804, 0];

q=sym('q_%d',[4, 1],'real');
alpha = sym('alpha_%d',[4, 1],'real');

for i=1:4
    T(:,:,i)=[
            cos(q(i)), -sin(q(i)), 0, a(i);
            sin(q(i)).*cos(alpha(i)), cos(q(i))*cos(alpha(i)), -sin(alpha(i)), -d(i)*sin(alpha(i));
            sin(q(i))*sin(alpha(i)), cos(q(i))*sin(alpha(i)), cos(alpha(i)), d(i)*cos(alpha(i));
            0,0,0,1];
end

T01=T(:,:,1);
T02=T01*T(:,:,2);
T03=T02*T(:,:,3);
T04=T03*T(:,:,4);

for (i=1: 4)
    for (j=1: 4)
        % t1
        u41(i, j) = jacobian(T04(i, j), q(1));
        u31(i, j) = jacobian(T03(i, j), q(1));
        u21(i, j) = jacobian(T02(i, j), q(1));
        u11(i, j) = jacobian(T01(i, j), q(1));
        
        % t2
        u42(i, j) = jacobian(T04(i, j), q(2));
        u32(i, j) = jacobian(T03(i, j), q(2));
        u22(i, j) = jacobian(T02(i, j), q(2));
        
        % t3
        u43(i, j) = jacobian(T04(i, j), q(3));
        u33(i, j) = jacobian(T03(i, j), q(3));
        
        % t4
        u44(i, j) = jacobian(T04(i, j), q(4));
    end
end

optimize = true;
matlabFunction(u41, 'File', 'autogen/J_ll_T04_q1', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u31, 'File', 'autogen/J_ll_T03_q1', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u21, 'File', 'autogen/J_ll_T02_q1', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u11, 'File', 'autogen/J_ll_T01_q1', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u42, 'File', 'autogen/J_ll_T04_q2', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u32, 'File', 'autogen/J_ll_T03_q2', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u22, 'File', 'autogen/J_ll_T02_q2', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u43, 'File', 'autogen/J_ll_T04_q3', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u33, 'File', 'autogen/J_ll_T03_q3', 'Vars',{q, alpha}, 'Optimize', optimize);
matlabFunction(u44, 'File', 'autogen/J_ll_T04_q4', 'Vars',{q, alpha}, 'Optimize', optimize);
%}

% CAD评估
m1_r1 = 0.42*[0.01;-0.01;0.01;1];
m2_r2 = 0.35*[0.0;-0.09;0.03;1];
m3_r3 = 0.66*[0.0;0.0;-0.11;1];
m4_r4 = 0.58*[-0.01;-0.21;0.02;1];

% 搜索参数 FAILED
%{
alpha = [-1.309, -pi/2, pi/2, -pi/2]';
data = load('data_l3/l_leg_new.txt');
sample_step = 3;
qs = data(:, 1:4);
tor = data(:, 9:12);
g = [0 0 -9.8 0]';

m1=0.42; r1=[0.01;-0.01;0.01];
m2=0.35; r2=[0.0;-0.09;0.03];
% m3=0.66; r3=[0.0;0.0;-0.11]; 
m3=0.86; r3=[0.005;0.01;-0.12]; 
% m4=0.58; r4=[-0.01;-0.21;0.02]; 
m4=0.58; r4=[-0.015;-0.1950;-0.03];

m1_s = m1-0.1:0.05:m1+0.1;
m2_s = m2-1.0:0.05:m2+0.0;
m3_s = m3-0.0:0.05:m3+8.0;
m4_s = m4-0.0:0.05:m4+0.8;

r1x_s = r1(1)-0.01:0.005:r1(1)+0.01;
r1y_s = r1(2)-0.01:0.005:r1(2)+0.01;
r1z_s = r1(3)-0.01:0.005:r1(3)+0.01;

r2x_s = r2(1)-0.00:0.005:r2(1)+0.0;
r2y_s = r2(2)-0.1:0.005:r2(2)+0;
r2z_s = r2(3)-0.1:0.005:r2(3)+0;

r3x_s = r3(1)-0.01:0.005:r3(1)+0.01;
r3y_s = r3(2)-0.01:0.005:r3(2)+0.01;
r3z_s = r3(3)-0.01:0.005:r3(3)+0.0;

r4x_s = r4(1)-0.05:0.005:r4(1)+0.05;
r4y_s = r4(2)-0.05:0.005:r4(2)+0.05;
r4z_s = r4(3)-0.02:0.005:r4(3)+0.0;

min_cost = 9999;
best_est = [];all_costs = [];
cnt=0;


% for l_1 = 1:length(m1_s)
for l_2 = 1:length(m2_s)
% for l_3 = 1:length(m3_s)
% for l_4 = 1:length(m4_s)

% for l_5 = 1:length(r1x_s)
% for l_6 = 1:length(r1y_s)
% for l_7 = 1:length(r1z_s)
%     
for l_8 = 1:length(r2x_s)
for l_9 = 1:length(r2y_s)
for l_10 = 1:length(r2z_s)
% 
% for l_11 = 1:length(r3x_s)
% for l_12 = 1:length(r3y_s)
% for l_13 = 1:length(r3z_s)

% for l_14 = 1:length(r4x_s)
% for l_15 = 1:length(r4y_s)
% for l_16 = 1:length(r4z_s)
    
%     disp([l_1, length(m1_s), l_2, length(m2_s), l_3, length(m3_s), l_4, length(m4_s)]);
%     disp("1:")
%     disp([l_5, length(r1x_s), l_6, length(r1y_s), l_7, length(r1z_s)]);
%     disp("2:")
%     disp([l_8, length(r2x_s), l_9, length(r2y_s), l_10, length(r2z_s)]);
%     disp("3:")
%     disp([l_11, length(r3x_s), l_12, length(r3y_s), l_13, length(r3z_s)]);
%     disp("4:")
%     disp([l_14, length(r4x_s), l_15, length(r4y_s), l_16, length(r4z_s)]);

%       disp([l_4, length(m4_s), l_14, length(r4x_s), l_15, length(r4y_s), l_16, length(r4z_s)]);
%      disp([l_3, length(m3_s), l_11, length(r3x_s), l_12, length(r3y_s), l_13, length(r3z_s)]);
     disp([l_2, length(m2_s), l_8, length(r2x_s), l_9, length(r2y_s), l_10, length(r2z_s)]);
%     m1 = m1_s(l_1);
    m2 = m2_s(l_2);
%      m3 = m3_s(l_3);
%     m4 = m4_s(l_4);
%     r1x = r1x_s(l_5);
%     r1y = r1y_s(l_5);
%     r1z = r1z_s(l_6);
    r2x = r2x_s(l_8);
    r2y = r2y_s(l_9);
    r2z = r2z_s(l_10);

%     r3x = r3x_s(l_11);
%     r3y = r3y_s(l_12);
%     r3z = r3z_s(l_13);

%     r4x = r4x_s(l_14);
%     r4y = r4y_s(l_15);
%     r4z = r4z_s(l_16);

%     m1_r1 = [m1*r1x; m1*r1y; m1*r1z; m1];
%     m2_r2 = [m2*r2x; m2*r2y; m2*r2z; m2];
    m3_r3 = [m3*r3(1); m3*r3(2); m3*r3(3); m3];m4_r4 = [m4*r4(1); m4*r4(2); m4*r4(3); m4];
%     m4_r4 = [m4*r4x; m4*r4y; m4*r4z; m4];

    tau_1s_real = [];tau_2s_real = [];tau_3s_real = [];tau_4s_real = [];
    tau_1s_est = [];tau_2s_est = [];tau_3s_est = [];tau_4s_est = [];
    costs = [];
    for i=1:sample_step:length(qs)
        q = qs(i, :)'; 

        u11 = J_ll_T01_q1(q, alpha);u21 = J_ll_T02_q1(q, alpha);u31 = J_ll_T03_q1(q, alpha);u41 = J_ll_T04_q1(q, alpha);
        u22 = J_ll_T02_q2(q, alpha);u32 = J_ll_T03_q2(q, alpha);u42 = J_ll_T04_q2(q, alpha);
        u33 = J_ll_T03_q3(q, alpha);u43 = J_ll_T04_q3(q, alpha);
        u44 = J_ll_T04_q4(q, alpha);

%         tau_1 = -g'*u11*m1_r1-g'*u21*m2_r2-g'*u31*m3_r3-g'*u41*m4_r4;
        tau_2 = -g'*u22*m2_r2-g'*u32*m3_r3-g'*u42*m4_r4;
        tau_3 = -g'*u33*m3_r3-g'*u43*m4_r4;
        tau_4 = -g'*u44*m4_r4;

        % [-3.07000000000000;-1.74000000000000;-0.497998000000000;1.50000000000000]
        % [-5.21, 0.9357, 2.4768, 0.0097]    

        tau_4s_real = [tau_4s_real; tor(i, 4)];
        tau_3s_real = [tau_3s_real; tor(i, 3)];
        tau_2s_real = [tau_2s_real; tor(i, 2)];
%         tau_1s_real = [tau_1s_real; tor(i, 1)];

        tau_4s_est = [tau_4s_est; tau_4];
        tau_3s_est = [tau_3s_est; tau_3];
        tau_2s_est = [tau_2s_est; tau_2];
%         tau_1s_est = [tau_1s_est; tau_1];

%         cost = sqrt(mean((tau_1s_est-tau_1s_real).^2)) + sqrt(mean((tau_2s_est-tau_2s_real).^2)) +  sqrt(mean((tau_3s_est-tau_3s_real).^2)) +  sqrt(mean((tau_4s_est-tau_4s_real).^2));
        cost = sqrt(mean((tau_2s_est-tau_2s_real).^2));
        costs = [costs; cost];
    end

    if (mean(costs)<min_cost)
        min_cost = mean(costs);
%         best_est = [m1_r1,m2_r2,m3_r3,m4_r4];
        best_est = m2_r2;
        best_ids = [l_2,l_8,l_9,l_10];
    end

    cnt=cnt+1;
end
end
end
end
% end
% end
% end
% end
% end
% end
% end
% end
% end
% end
% end
% end

% 查看结果
% best_m4 = m4_s(best_ids(1));
% best_r4x = r4x_s(best_ids(2));
% best_r4y = r4y_s(best_ids(3));
% best_r4z = r4z_s(best_ids(4));

best_m2 = m2_s(best_ids(1));
best_r2x = r2x_s(best_ids(2));
best_r2y = r2y_s(best_ids(3));
best_r2z = r2z_s(best_ids(4));

tau_1s_real = [];tau_2s_real = [];tau_3s_real = [];tau_4s_real = [];
tau_1s_est = [];tau_2s_est = [];tau_3s_est = [];tau_4s_est = [];

% tau_4
for i=1:1:length(qs)
    q = qs(i, :)'; 
    
    u11 = J_ll_T01_q1(q, alpha);u21 = J_ll_T02_q1(q, alpha);u31 = J_ll_T03_q1(q, alpha);u41 = J_ll_T04_q1(q, alpha);
    u22 = J_ll_T02_q2(q, alpha);u32 = J_ll_T03_q2(q, alpha);u42 = J_ll_T04_q2(q, alpha);
    u33 = J_ll_T03_q3(q, alpha);u43 = J_ll_T04_q3(q, alpha);
    u44 = J_ll_T04_q4(q, alpha);
    %     
    tau_4 = -g'*u44*m4_r4;
    tau_3 = -g'*u33*m3_r3-g'*u43*m4_r4;
    tau_2 = -g'*u22*best_est-g'*u32*m3_r3-g'*u42*m4_r4;
    
    tau_4s_real = [tau_4s_real; tor(i, 4)];
    tau_4s_est = [tau_4s_est; tau_4];
    tau_3s_real = [tau_3s_real; tor(i, 3)];
    tau_3s_est = [tau_3s_est; tau_3];
    tau_2s_real = [tau_2s_real; tor(i, 2)];
    tau_2s_est = [tau_2s_est; tau_2];
end

figure(2);
plot(tau_2s_real, "r.-");
hold on;
plot(tau_2s_est, "g.-");
figure(3);
plot(tau_3s_real, "r.-");
hold on;
plot(tau_3s_est, "g.-");
figure(4);
plot(tau_4s_real, "r.-");
hold on;
plot(tau_4s_est, "g.-");
legend('real', 'est');   
%}

%{
m1_r1 = [m1*r1(1); m1*r1(2); m1*r1(3); m1];
m2_r2 = [m2*r2(1); m2*r2(2); m2*r2(3); m2];
m3_r3 = [m3*r3(1); m3*r3(2); m3*r3(3); m3];
m4_r4 = [m4*r4(1); m4*r4(2); m4*r4(3); m4];
    
tau_1s_real = [];tau_2s_real = [];tau_3s_real = [];tau_4s_real = [];
tau_1s_est = [];tau_2s_est = [];tau_3s_est = [];tau_4s_est = [];

for i=1:1:length(qs)
    q = qs(i, :)'; 

    u11 = J_ll_T01_q1(q, alpha);u21 = J_ll_T02_q1(q, alpha);u31 = J_ll_T03_q1(q, alpha);u41 = J_ll_T04_q1(q, alpha);
    u22 = J_ll_T02_q2(q, alpha);u32 = J_ll_T03_q2(q, alpha);u42 = J_ll_T04_q2(q, alpha);
    u33 = J_ll_T03_q3(q, alpha);u43 = J_ll_T04_q3(q, alpha);
    u44 = J_ll_T04_q4(q, alpha);
    
    tau_1 = -g'*u11*m1_r1-g'*u21*m2_r2-g'*u31*m3_r3-g'*u41*m4_r4;
    tau_2 = -g'*u22*m2_r2-g'*u32*m3_r3-g'*u42*m4_r4;
    tau_3 = -g'*u33*m3_r3-g'*u43*m4_r4;
    tau_4 = -g'*u44*m4_r4;

    % [-3.07000000000000;-1.74000000000000;-0.497998000000000;1.50000000000000]
    % [-5.21, 0.9357, 2.4768, 0.0097]    

    tau_4s_real = [tau_4s_real; tor(i, 4)];
    tau_3s_real = [tau_3s_real; tor(i, 3)];
    tau_2s_real = [tau_2s_real; tor(i, 2)];
    tau_1s_real = [tau_1s_real; tor(i, 1)];

    tau_4s_est = [tau_4s_est; tau_4];
    tau_3s_est = [tau_3s_est; tau_3];
    tau_2s_est = [tau_2s_est; tau_2];
    tau_1s_est = [tau_1s_est; tau_1];
end

figure(1);
plot(tau_1s_real, "r.-");
hold on;
plot(tau_1s_est, "g.-");
figure(2);
plot(tau_2s_real, "r.-");
hold on;
plot(tau_2s_est, "g.-");
figure(3);
plot(tau_3s_real, "r.-");
hold on;
plot(tau_3s_est, "g.-");
figure(4);
plot(tau_4s_real, "r.-");
hold on;
plot(tau_4s_est, "g.-");
legend('real', 'est');   
%}

% 部署的参数
% m1_r1 = [-0.571796767277893;-0.00732115175136323;0;0];
% m2_r2 = [-0.223097970610209;-0.0724328733432290;-0.00732115175136323;-0.000106156700394766];
% m3_r3 = [0.0758990177329540;0.0302522904468216;0.0724328733432290;0.0279917715528874];
% m4_r4 = [-0.0214620345127087;-0.173788966340042;0.0302522904468216;0.0279917715528874];

% m1_r1 = [-0.431328002537869;0.0606093881109318;0;0];
% m2_r2 = [-0.206140042034996;-0.0887949076686677;0.0606093881109318;0.000878836127608511];
% m3_r3 = [0.0350878510311750;0.0186934101812546;0.0887949076686677;0.0353265664976769];
% m4_r4 = [0.00402612856367001;0.171051783674433;-0.0186934101812547;-0.0353265664976770];

% 加载最新估计出来的参数
% load('est.mat');
% m4_r4 = -est(13:16);
% m3_r3 = -est(9:12);
% m2_r2 = -est(5:8);
% m1_r1 = -est(1:4);

% m1_r1 = -[0.440200322572276;-0.0533243839981347;0;0];
% m2_r2 = -[0.196382663728450;0.0897861536068708;-0.0533243839981345;-0.000773203567972903];
% m3_r3 = -[-0.0285241149034225;-0.0185402785398706;-0.0897861536068707;-0.0356059673070341];
% m4_r4 = -[0.00232813091490391;0.170696141746285;-0.0185402785398706;-0.0356059673070341];

% m1_r1 = -[0.471627015563349;-0.0326650499846719;0;0];
% m2_r2 = -[0.201959023704279;0.0839414367711595;0;0];
% m3_r3 = -[-0.0590809149664679;-0.00997243202318335;-0.0839414367711595;-0.0325645567637326];
% m4_r4 = -[0.0164831017809083;0.159974104402136;-0.00997243202318336;-0.0325645567637326];

% 3、最小二乘估计参数
alpha = [-1.309, -pi/2, pi/2, -pi/2]';

data = load('data_l3/l_leg_new.txt');
data = data(:, :);

qs = data(:, 1:4);
tor = data(:, 9:12);

g = [0 0 -9.8 0]';

K = [];
Tau = [];
tau_1s_real = [];tau_2s_real = [];tau_3s_real = [];tau_4s_real = [];
tau_1s_est = [];tau_2s_est = [];tau_3s_est = [];tau_4s_est = [];
costs = [];
for i=1:1:length(qs)
    q = qs(i, :)'; 
    
    u11 = J_ll_T01_q1(q, alpha);u21 = J_ll_T02_q1(q, alpha);u31 = J_ll_T03_q1(q, alpha);u41 = J_ll_T04_q1(q, alpha);
    u22 = J_ll_T02_q2(q, alpha);u32 = J_ll_T03_q2(q, alpha);u42 = J_ll_T04_q2(q, alpha);
    u33 = J_ll_T03_q3(q, alpha);u43 = J_ll_T04_q3(q, alpha);
    u44 = J_ll_T04_q4(q, alpha);
    
    %     
    tau_1 = -g'*u11*m1_r1-g'*u21*m2_r2-g'*u31*m3_r3-g'*u41*m4_r4;
    tau_2 = -g'*u22*m2_r2-g'*u32*m3_r3-g'*u42*m4_r4;
    tau_3 = -g'*u33*m3_r3-g'*u43*m4_r4;
    tau_4 = -g'*u44*m4_r4;
    
    % [-3.07000000000000;-1.74000000000000;-0.497998000000000;1.50000000000000]
    % [-5.21, 0.9357, 2.4768, 0.0097]    
    
    tau_4s_real = [tau_4s_real; tor(i, 4)];
    tau_3s_real = [tau_3s_real; tor(i, 3)];
    tau_2s_real = [tau_2s_real; tor(i, 2)];
    tau_1s_real = [tau_1s_real; tor(i, 1)];
    
    tau_4s_est = [tau_4s_est; tau_4];
    tau_3s_est = [tau_3s_est; tau_3];
    tau_2s_est = [tau_2s_est; tau_2];
    tau_1s_est = [tau_1s_est; tau_1];
    
    cost = sqrt(mean((tau_1s_est-tau_1s_real).^2)) + sqrt(mean((tau_2s_est-tau_2s_real).^2)) +  sqrt(mean((tau_3s_est-tau_3s_real).^2)) +  sqrt(mean((tau_4s_est-tau_4s_real).^2));
    costs = [costs; cost];
    
        
    O = [0, 0, 0, 0];
    Kj = [
%         g'*u11, g'*u21, g'*u31, g'*u41;
        O, g'*u22, g'*u32, g'*u42;
        O, O, g'*u33, g'*u43;
        O, O, O, g'*u44;
        ];
    
    K = [K; Kj];
    Tau = [Tau; 
%         tor(i, 1)'; 
        tor(i, 2)'; 
        tor(i, 3)'; 
        tor(i, 4)'
        ];
end

% 最小二乘估计 K*Tau=est
A = K'*K;
est1 = K'*pinv(K*K')*Tau;
rk = rank(A);
est2 = pinv(A)*(K'*Tau);
est3 = pinv(K)*Tau;
% est = linsolve(K, Tau);
% save('est.mat', 'est');

% 
figure(1);
plot(tau_1s_real, "r.-");
hold on;
plot(tau_1s_est, "g.-");
figure(2);
plot(tau_2s_real, "r.-");
hold on;
plot(tau_2s_est, "g.-");
figure(3);
plot(tau_3s_real, "r.-");
hold on;
plot(tau_3s_est, "g.-");
figure(4);
plot(tau_4s_real, "r.-");
hold on;
plot(tau_4s_est, "g.-");
legend('real', 'est');   



